{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor_intro_md"
      },
      "source": [
        "# Factor"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor_desc_md"
      },
      "source": [
        "`gtsam.Factor` is the abstract base class for all factors in GTSAM, including nonlinear factors, Gaussian factors, discrete factors, and conditionals. It defines the basic interface common to all factors, primarily centered around the set of variables (keys) the factor involves.\n",
        "\n",
        "You typically do not instantiate `gtsam.Factor` directly but rather work with its derived classes like `gtsam.NonlinearFactor`, `gtsam.JacobianFactor`, `gtsam.DiscreteFactor`, etc.\n",
        "\n",
        "The `error` function of a factor is typically related to a probability or likelihood $P(X)$ or $\\phi(X)$ via the negative log-likelihood:\n",
        "\n",
        "$$\n",
        "\\text{error}(X) = - \\log \\phi(X) + K\n",
        "$$\n",
        "or equivalently:\n",
        "$$\n",
        "\\phi(X) \\propto \\exp(-\\text{error}(X))\n",
        "$$\n",
        "where $X$ are the variables involved in the factor, $\\phi(X)$ is the potential function (proportional to probability or likelihood), and $K$ is some constant. Minimizing the error corresponds to maximizing the probability/likelihood represented by the factor."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor_colab_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/inference/doc/Factor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "factor_pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "%pip install --quiet gtsam-develop"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "factor_import_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "from gtsam.utils.test_case import GtsamTestCase\n",
        "\n",
        "# We need a concrete factor type for demonstration\n",
        "from gtsam import PriorFactorPose2, BetweenFactorPose2, Pose2, Point3\n",
        "from gtsam import symbol_shorthand\n",
        "\n",
        "X = symbol_shorthand.X"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor_interface_md"
      },
      "source": [
        "## Basic Interface\n",
        "\n",
        "All factors provide methods to access the keys of the variables they involve."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "factor_keys_code",
        "outputId": "01234567-89ab-cdef-0123-456789abcdef"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Prior factor keys: [8646911284551352320] (x0)\n",
            "Prior factor size: 1\n",
            "Between factor keys: [8646911284551352320, 8646911284551352321] (x0, x1)\n",
            "Between factor size: 2\n",
            "Is prior factor empty? False\n",
            "Prior Factor: PriorFactor on x0\n",
            "  prior mean:  (0, 0, 0)\n",
            "  noise model: diagonal sigmas [0.1; 0.1; 0.05];\n"
          ]
        }
      ],
      "source": [
        "noise_model = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.1, 0.1, 0.05))\n",
        "\n",
        "# Create some concrete factors\n",
        "prior_factor = PriorFactorPose2(X(0), Pose2(0, 0, 0), noise_model)\n",
        "between_factor = BetweenFactorPose2(X(0), X(1), Pose2(1, 0, 0), noise_model)\n",
        "\n",
        "# Access keys (methods inherited from gtsam.Factor)\n",
        "prior_keys = prior_factor.keys()\n",
        "print(f\"Prior factor keys: {prior_keys} ({gtsam.DefaultKeyFormatter(prior_keys[0])})\")\n",
        "print(f\"Prior factor size: {prior_factor.size()}\")\n",
        "\n",
        "between_keys = between_factor.keys()\n",
        "print(f\"Between factor keys: {between_keys} ({gtsam.DefaultKeyFormatter(between_keys[0])}, {gtsam.DefaultKeyFormatter(between_keys[1])})\")\n",
        "print(f\"Between factor size: {between_factor.size()}\")\n",
        "\n",
        "print(f\"Is prior factor empty? {prior_factor.empty()}\")\n",
        "\n",
        "# Factors can be printed\n",
        "prior_factor.print(\"Prior Factor: \")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor_error_md"
      },
      "source": [
        "## Error Function\n",
        "\n",
        "A key method for many factor types (especially nonlinear and Gaussian) is `error(Values)`. This evaluates the negative log-likelihood of the factor given a specific assignment of variable values. For optimization, the goal is typically to find the `Values` that minimize the total error (sum of errors from all factors)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "factor_error_code",
        "outputId": "12345678-9abc-def0-1234-56789abcdef0"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Error at ground truth: 0.0\n",
            "Error with incorrect x1: 50.0\n"
          ]
        }
      ],
      "source": [
        "values = gtsam.Values()\n",
        "values.insert(X(0), Pose2(0, 0, 0))\n",
        "values.insert(X(1), Pose2(1, 0, 0))\n",
        "\n",
        "# Evaluate error (example with BetweenFactor)\n",
        "error1 = between_factor.error(values)\n",
        "print(f\"Error at ground truth: {error1}\")\n",
        "\n",
        "# Change a value and recalculate error\n",
        "values.update(X(1), Pose2(0, 0, 0))\n",
        "error2 = between_factor.error(values)\n",
        "print(f\"Error with incorrect x1: {error2:.1f}\")"
      ]
    }
  ],
  "metadata": {
    "colab": {
      "provenance": []
    },
    "kernelspec": {
      "display_name": "gtsam",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.13.1"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}